/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#include "rtthread.h"
#include "combos.h"
#include "camera_rc.h"

#define MAX_COBMBOS_COUNT 50
combos_s COMBOS_0_profile;
vehicleCtrlCmdNew_s COMBOS_0_CMD[MAX_COBMBOS_COUNT];
struct program_cmd
{
    int16_t throttle;
    int16_t turn;
    int16_t shitf;
    uint16_t turretPitch;
    uint16_t fire;
    uint16_t moveTime;
    uint16_t fireTime;
} pro_cmd;
void program_vehicleWheelCtrl(int16_t throttle, // 前进+/后退-
                              int16_t shitf,    // 左移-/左移+
                              int16_t turn      // 左转-/右转+

)
{
    pro_cmd.throttle = throttle;
    pro_cmd.turn = turn;
}
void program_gunturretCtrl(uint8_t gunPitch, uint8_t fire)
{
    pro_cmd.turretPitch = gunPitch;
}
void set_combos_0_cmd(uint8_t index,
                      uint8_t maxIndex,
                      int16_t throttle,
                      int16_t turn,
                      int16_t shitf,
                      uint16_t turretPitch,
                      uint16_t fire,
                      uint16_t moveTime,
                      uint16_t fireTime)
{
    if (index < MAX_COBMBOS_COUNT)
    {
        COMBOS_0_CMD[index].throttle = throttle;
        COMBOS_0_CMD[index].turn = turn;
        COMBOS_0_CMD[index].shitf = shitf;
        COMBOS_0_CMD[index].turretPitch = turretPitch;
        COMBOS_0_CMD[index].fire = fire;
        COMBOS_0_CMD[index].moveTime = moveTime;
        COMBOS_0_CMD[index].fireTime = fireTime;
        COMBOS_0_profile.cmdUsed = maxIndex;
    }
    // rt_kprintf("index:%d/%d,throttle:%d,turn:%d,shitf:%d,",index, maxIndex, throttle, turn, shitf);
    // rt_kprintf("turretPitch:%d,fire:%d,moveTime:%d,fireTime:%d\n", turretPitch, fire, moveTime, fireTime);
}

void clearVehicleBlocklyMotion()
{
    rt_memset(COMBOS_0_CMD, 0, sizeof(COMBOS_0_CMD));
}
void camera_program_init(void)
{
    vehicelCmdInit(&COMBOS_0_profile, COMBOS_0_CMD, 50, 1);
}
/**
 * @brief 摄像头编程控制循环，必须10ms调用一次
 *
 */
void camera_program_loop(void)
{
    static bool unlockFlag = false;

    if (is_camera_combos_trig())
    {
        vehicelCmdSetState(&COMBOS_0_profile, COMBOS_RESTART);
    }
    if (is_camera_unlock_trig())
    {
        unlockFlag = true;
    }
    if (is_camera_lock_trig())
    {
        unlockFlag = false;
    }

    vehicelCmdRun(&COMBOS_0_profile);
    if (unlockFlag)
    {
        // vehicelCmdSetState(&COMBOS_0_profile, COMBOS_RUN);
        // rt_kprintf("pro_cmd: thr: %d turn: %d pitch:%d  vT:%d\n", pro_cmd.throttle, pro_cmd.turn, pro_cmd.turretPitch,pro_cmd.moveTime);
    }
}
